//
// Created by wzc on 2023/2/7.
// kinodynamic astar的测试文件
//

#include <ros/ros.h>
#include <chrono>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>

#include "plan_env/sdf_map.h"
#include "plan_env/map_ros.h"
#include "plan_env/edt_environment.h"

#include "path_searching/kinodynamic_astar.h"

Eigen::Vector3d start_pt_;
Eigen::Vector3d goal_pt_;

Eigen::Vector3d start_vel_;
Eigen::Vector3d goal_vel_;

Eigen::Vector3d start_acc_;

using namespace fast_planner;

shared_ptr<SDFMap> sdf_map_;
EDTEnvironment::Ptr edt_environment_;
unique_ptr<KinodynamicAstar> kino_path_finder_;

ros::Publisher path_pub_, path_sample_pub_, visited_pt_pub_;

nav_msgs::Path path_, path_sample_;
vector<Eigen::Vector3d> path_vector_;

vector<PathNodePtr> visited_pt_;

vector<Eigen::Vector3d> point_set, start_end_derivatives;

void init(ros::NodeHandle& nh){
  sdf_map_.reset(new SDFMap);
  sdf_map_->initMap(nh);

  edt_environment_.reset(new EDTEnvironment);
  edt_environment_->setMap(sdf_map_);

  kino_path_finder_.reset(new KinodynamicAstar);
  kino_path_finder_->setParam(nh);
  kino_path_finder_->setEnvironment(edt_environment_);
  kino_path_finder_->init();

  sdf_map_->setGlobalKnownFree();

  Eigen::Vector3d obs_min;
  Eigen::Vector3d obs_max;

  /// surroundings
  obs_min = Eigen::Vector3d(-20.0, -10.0, -0.5);
  obs_max = Eigen::Vector3d(20.0, -9.5, 3.0);
  sdf_map_->setObs(obs_min, obs_max);

  obs_min = Eigen::Vector3d(-20.0, 9.5, -0.5);
  obs_max = Eigen::Vector3d(20.0, 10.0, 3.0);
  sdf_map_->setObs(obs_min, obs_max);

  obs_min = Eigen::Vector3d(-20.0, -10.0, -0.5);
  obs_max = Eigen::Vector3d(-19.5, 10.0, 3.0);
  sdf_map_->setObs(obs_min, obs_max);

  obs_min = Eigen::Vector3d(19.5, -10.0, -0.5);
  obs_max = Eigen::Vector3d(20.0, 10.0, 3.0);
  sdf_map_->setObs(obs_min, obs_max);

  Eigen::Vector2d obs_center, obs_size;
  obs_size = Eigen::Vector2d(2.0, 2.0);

  obs_center = Eigen::Vector2d(9.0, 6.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(5.0, 0.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(13.0, -1.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(0.0, -4.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(-2.0, 3.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(-10.0, 5.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(-6.0, -1.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  obs_center = Eigen::Vector2d(-16.0, -0.0);
  sdf_map_->setObsWithCenter(obs_center, obs_size);

  sdf_map_->updateGlobalESDF3d();
  sdf_map_->setAllBoundToGlobal();


  /// 预设初末位置
  // ！！！！ 如果初始都是va都是0，搜索会失败，但是重启后会好 ！！！！
  start_pt_ << 0.0, 0.0, 0.6;
  start_vel_ << 0.0, 0.0, 0.0;
  start_acc_ << 0.0, 0.0, 0.0;

  goal_pt_ << 8.0, 3.0, 1.5;
  goal_vel_ << 0.0, 0.0, 0.0;

  path_.header.frame_id = "world";
  path_sample_.header.frame_id = "world";
}

void goalCallback(const geometry_msgs::PoseStampedConstPtr& goal){
  std::cout << "[Main]in the goal call back" << std::endl;
  goal_pt_[0] = goal->pose.position.x;
  goal_pt_[1] = goal->pose.position.y;
  goal_pt_[2] = 1.8;

  std::cout << "[Main]start pt " << start_pt_.transpose() << std::endl;
  std::cout << "[Main]start vel " << start_vel_.transpose() << std::endl;
  std::cout << "[Main]start acc " << start_acc_.transpose() << std::endl;

  std::cout << "[Main]goal pt " << goal_pt_.transpose() << std::endl;
  std::cout << "[Main]goal vel " << goal_vel_.transpose() << std::endl;

  auto start = chrono::system_clock::now();

  kino_path_finder_->reset();
  int status = kino_path_finder_->search(start_pt_,start_vel_, start_acc_, goal_pt_,goal_vel_, true);
  if (status == KinodynamicAstar::NO_PATH) {
    ROS_WARN_STREAM("RETRY!");
    status = kino_path_finder_->search(start_pt_,start_vel_, start_acc_, goal_pt_,goal_vel_, false);
  }

  auto end = chrono::system_clock::now();
  auto duration = chrono::duration_cast<chrono::microseconds>(end - start);
  ROS_WARN_STREAM("[Main]time consumption "
                    << double(duration.count()) * chrono::microseconds::period::num / chrono::microseconds::period::den * 1000.0
                    << "ms");

  if (status == KinodynamicAstar::NO_PATH) {
    ROS_ERROR("[Main]search 1 fail");
    ROS_WARN_STREAM("[Main]searchKinoPath done!");
    return;
  }

  path_vector_.clear();
  path_vector_ = kino_path_finder_->getKinoTraj(0.01);
  std::cout << "[Main]end pt of path is : " << path_vector_[path_vector_.size()-1].transpose() << std::endl;

  path_.poses.clear();
  for(auto & i : path_vector_){
    geometry_msgs::PoseStamped pt;
    pt.pose.position.x = i(0);
    pt.pose.position.y = i(1);
    pt.pose.position.z = i(2);
    pt.pose.orientation.x = 0;
    pt.pose.orientation.y = 0;
    pt.pose.orientation.z = 0;
    pt.pose.orientation.w = 1;

    path_.poses.push_back(pt);
  }

  visited_pt_.clear();
  visited_pt_ =kino_path_finder_->getVisitedNodes();

//  std::cout << "[Main] duration " << kino_path_finder_->getTrajDuration() << std::endl;

  double ts = 0.3;
  point_set.clear();
  start_end_derivatives.clear();
  kino_path_finder_->getSamples(ts, point_set, start_end_derivatives);

  std::cout << "[Main] start v " << start_end_derivatives[0].transpose() << std::endl;
  std::cout << "[Main] end v " << start_end_derivatives[1].transpose() << std::endl;
  std::cout << "[Main] start a " << start_end_derivatives[2].transpose() << std::endl;
  std::cout << "[Main] end a " << start_end_derivatives[3].transpose() << std::endl;

  path_sample_.poses.clear();
  for(auto & i : point_set){
    geometry_msgs::PoseStamped pt;
    pt.pose.position.x = i(0);
    pt.pose.position.y = i(1);
    pt.pose.position.z = i(2);
    pt.pose.orientation.x = 0;
    pt.pose.orientation.y = 0;
    pt.pose.orientation.z = 0;
    pt.pose.orientation.w = 1;

    path_sample_.poses.push_back(pt);
  }

  ROS_WARN_STREAM("[Main]searchKinoPath done!");
}

void timerCallback(const ros::TimerEvent&){
  path_pub_.publish(path_);
  path_sample_pub_.publish(path_sample_);

  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::PointXYZ pt;
  for(int i = 0; i < visited_pt_.size(); i++){
    pt.x = visited_pt_[i]->state[0];
    pt.y = visited_pt_[i]->state[1];
    pt.z = visited_pt_[i]->state[2];
    cloud.push_back(pt);
  }
  cloud.width = cloud.points.size();
  cloud.height = 1;
  cloud.is_dense = true;
  cloud.header.frame_id = "world";
  sensor_msgs::PointCloud2 cloud_msg;
  pcl::toROSMsg(cloud, cloud_msg);

  visited_pt_pub_.publish(cloud_msg);
}



int main(int argc, char** argv)
{
  ros::init(argc,argv,"kinodynamic_astar_test");
  ros::NodeHandle nh("~");

  init(nh);

  ros::Subscriber goal_sub = nh.subscribe("/move_base_simple/goal",10,goalCallback);

  ros::Timer path_timer = nh.createTimer(ros::Duration(0.2), timerCallback);

  path_pub_ = nh.advertise<nav_msgs::Path>("/kinodynamic_astar/path",10);
  path_sample_pub_ = nh.advertise<nav_msgs::Path>("/kinodynamic_astar/path_sample",10);
  visited_pt_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/kinodynamic_astar/visited_pt",10);

  ros::spin();
}
